#include "PhysicsObjects.h"
#include "CollisionTest.h"
PhysicsBox::PhysicsBox() : BaseIndexedGraphicalEntity(), Angles(0,0,0)
{
	BoundingBox.Position = D3DXVECTOR3(0,0,0);
	BoundingBox.HalfVector = D3DXVECTOR3(0,0,0);
	SetPosition(D3DXVECTOR3(0,0,0));
	SetVelocity(D3DXVECTOR3(0,0,0));
	D3DXMatrixIdentity(&RotMat);
}

PhysicsBox::PhysicsBox(IDirect3DDevice9 *nDevice) : BaseIndexedGraphicalEntity(nDevice), Angles(0,0,0)
{
	BoundingBox.Position = D3DXVECTOR3(0,0,0);
	BoundingBox.HalfVector = D3DXVECTOR3(0,0,0);
	SetPosition(D3DXVECTOR3(0,0,0));
	SetVelocity(D3DXVECTOR3(0,0,0));
	D3DXMatrixIdentity(&RotMat);

#ifdef _DEBUG
	TestBox = new BaseIndexedGraphicalEntity( gd3dDevice );
#endif
}

PhysicsBox::PhysicsBox(IDirect3DDevice9 *nDevice, D3DXVECTOR3 nPosition, D3DXVECTOR3 nVelocity) 
	: BaseIndexedGraphicalEntity(nDevice), Angles(0,0,0)
{
	BoundingBox.Position = D3DXVECTOR3(0,0,0);
	BoundingBox.HalfVector = D3DXVECTOR3(0,0,0);
	SetPosition(nPosition);
	SetVelocity(nVelocity);
	D3DXMatrixIdentity(&RotMat);

#ifdef _DEBUG
	TestBox = new BaseIndexedGraphicalEntity( gd3dDevice );
#endif
}

PhysicsBox::~PhysicsBox()
{
#ifdef _DEBUG
	if(TestBox)
		delete TestBox;
#endif
}

void PhysicsBox::InitCollisionBox(D3DXVECTOR3 nHalfVector)
{
	BoundingBox.Position = this->Position;
	BoundingBox.InitBox(nHalfVector);
	BoundingBox.HalfVector = nHalfVector;

#ifdef _DEBUG
	TestBox->CreateCube(D3DXVECTOR3(BoundingBox.HalfVector.x+0.001f, BoundingBox.HalfVector.y+0.001f, BoundingBox.HalfVector.z+0.001f));
#endif

}

void PhysicsBox::InitCollisionBox(float nX, float nY, float nZ)
{
	BoundingBox.Position = this->Position;
	BoundingBox.HalfVector = D3DXVECTOR3(nX, nY, nZ);
#ifdef _DEBUG
	if(Device)
		TestBox->CreateCube(D3DXVECTOR3(BoundingBox.HalfVector.x+0.00001f, BoundingBox.HalfVector.y+0.00001f, BoundingBox.HalfVector.z+0.00001f));
#endif
}
D3DXVECTOR3 PhysicsBox::GetHalfVector( void )
{
	return BoundingBox.HalfVector;
}

void PhysicsBox::SetHalfVector(D3DXVECTOR3 nHalfVector)
{
	BoundingBox.HalfVector = nHalfVector;
	BoundingBox.InitBox(nHalfVector);
	
}

/*bool PhysicsBox::CheckCollision(PhysicsBox *OtherBox)
{
	return CollisionTest::IsColliding(this->BoundingBox, OtherBox->BoundingBox);
}*/

void PhysicsBox::Update(float dt)
{
#ifdef _DEBUG
	TestBox->SetPosition(this->Position);
	TestBox->SetVelocity(D3DXVECTOR3(0,0,0));
	TestBox->Update(dt);
#endif
	//float l1 = 0, l2= 0;
	BaseIndexedGraphicalEntity::Update(dt);
	BoundingBox.Position = this->Position;

	D3DXMatrixRotationYawPitchRoll(&RotMat, Angles.x, Angles.y, Angles.z);
	//D3DXVECTOR3 NewHalfVector = GetHalfVector();
	//D3DXVECTOR3 OldHalfVector = GetHalfVector();

	for(UINT i = 0; i < BoundingBox.Points.size()-8; ++i)
	{
		D3DXVec3TransformCoord(&BoundingBox.Points[i],&BoundingBox.Points[i+8], &RotMat);
	}

	//D3DXVec3TransformCoord(&NewHalfVector, &NewHalfVector, &RotMat);
	//l1 = D3DXVec3Length(&NewHalfVector);
	//l2 = D3DXVec3Length(&OldHalfVector);
	//SetHalfVector(D3DXVECTOR3(NewHalfVector.x * OldHalfVector.x,NewHalfVector.y * OldHalfVector.y,NewHalfVector.z * OldHalfVector.z));
	//SetHalfVector(D3DXVECTOR3(abs(NewHalfVector.x),abs(NewHalfVector.y),abs(NewHalfVector.z)));
}

void PhysicsBox::Render(void)
{
	//DEBUG CODE
#ifdef _DEBUG
	gd3dDevice->SetRenderState(D3DRS_FILLMODE,D3DFILL_WIREFRAME);
	TestBox->Render();
	gd3dDevice->SetRenderState(D3DRS_FILLMODE,D3DFILL_SOLID);
#endif

	BaseIndexedGraphicalEntity::Render();
}

CollisionBox PhysicsBox::GetPhysbox( void )
{
	return BoundingBox;
}

void PhysicsBox::SetAngles (D3DXVECTOR3 nAngles)
{
	Angles = nAngles;
}

D3DXVECTOR3 PhysicsBox::GetAngles( void )
{
	return Angles;
}